#include "Grid.h"
#include <cmath>
#include <iostream>

#include "Particle.h"

extern int totalNumberOfParticles;
extern Particle** particles;
extern float worldSize;
extern float particleRadius;

int gridSize;
int xStride;
int yStride;

void createGrid() {

	voxelSize = 2.00f*particleRadius;
	gridLength = (int)ceil(2.0f * worldSize / voxelSize);
	/* 2 extra grid voxels in each dimension in case particles go slightly outside the world borders */
	gridLength += 2; 

	xStride = gridLength*gridLength;
	yStride = gridLength;

	std::cout << "Grid dimensions: " << gridLength << "*" << gridLength << "*" << gridLength << std::endl;

	gridMinimumPosition = -worldSize - voxelSize;
	std::cout << "Grid minimum position: " << gridMinimumPosition << std::endl;
	std::cout << "Grid maximum position: " << gridMinimumPosition + gridLength*voxelSize << std::endl;

	std::cout << "Grid voxel size: " << voxelSize << std::endl;

	gridSize = gridLength * gridLength * gridLength;

	indexGrid = new int[gridSize*particlesPerVoxel]; 
	countGrid = new int[gridSize];

	for (int i=0; i<gridSize*particlesPerVoxel; i++) {
		indexGrid[i] = -1;
	}

	for (int i=0; i<gridSize; i++) {
		countGrid[i] = 0;
	}
}

void updateGrid() {
	for (int i=0; i<gridSize*particlesPerVoxel; i++) {
		indexGrid[i] = -1;
	}
	for (int i=0; i<gridSize; i++) {
		countGrid[i] = 0;
	}

	for (int i=0; i<totalNumberOfParticles; i++) {

		particles[i]->updateGridIndex();
		int* gridIndex = particles[i]->getGridIndex();

		if (isValidIndex(gridIndex)) {
			int flatCountGridIndex = gridIndex[0]*xStride + gridIndex[1]*yStride + gridIndex[2];
			int flatIndexGridIndex = flatCountGridIndex * particlesPerVoxel;

			indexGrid[flatIndexGridIndex + countGrid[flatCountGridIndex]] = i;

			countGrid[flatCountGridIndex]++;
		}
	}
}

bool isValidIndex(int* gridIndex) {

	bool validIndex = true;
	for (int j=0; j<3; j++) {
		if (gridIndex[j] >= gridLength || gridIndex[j] < 0) {
			validIndex = false;
		}
	}
	return validIndex;
}

/* gridIndex must be a valid index! */
int* getNeighborParticleIndices(int* gridIndex) {

	for (int i=0; i<3; i++) {
		/* If you're at the border, go 1 cell inwards to prevent going out of bounds */
		if (gridIndex[i] == 0) { 
			gridIndex[i]++;
		} else if (gridIndex[i] == (gridLength-1)) {
			gridIndex[i]--;
		}
	}

	int checkIndex[3] = {gridIndex[0]-1,gridIndex[1]-1,gridIndex[2]-1};

	int neighborCount = 0;
	int* neighborParticleIndices = new int[27*particlesPerVoxel];
	
	for (int x=0; x<3; x++) {
		checkIndex[1] = gridIndex[1]-1; //reset y index before y-loop

		for (int y=0; y<3; y++) {
			checkIndex[2] = gridIndex[2]-1; //reset z index before z-loop

			for (int z=0; z<3; z++) {
				int flatCountGridIndex = checkIndex[0]*xStride + checkIndex[1]*yStride + checkIndex[2];
				int flatIndexGridIndex = flatCountGridIndex * particlesPerVoxel;

				neighborParticleIndices[neighborCount] = indexGrid[flatIndexGridIndex];
				neighborParticleIndices[neighborCount+1] = indexGrid[flatIndexGridIndex+1];
				neighborParticleIndices[neighborCount+2] = indexGrid[flatIndexGridIndex+2];
				neighborParticleIndices[neighborCount+3] = indexGrid[flatIndexGridIndex+3];

				neighborCount += 4;
				checkIndex[2]++;
			}
			checkIndex[1]++;
		}
		checkIndex[0]++;
	}
	return neighborParticleIndices;
}
